#include "sonar.h"
#include <iostream>
#include <fstream>
#include <string>
#include <string.h>
#include <sstream>
#include <vector>
#include <iomanip>
#include <fcntl.h>
#include <termios.h>
#include <unistd.h>
#include <mutex>
#include <condition_variable>
#include <thread>



#ifdef SONAR_TYPE1
bool validateChecksum(char buffer[]);

size_t bytesRead = 0;
Sonar::Sonar(const std::string& sensor_name_, const int& run_mode_, const std::string& dev_name_) : SensorAbstraction(sensor_name_, run_mode_) {
    dev_name = dev_name_;
    p_sonar_data = std::make_unique<struct sonar_data>();
}

Sonar::~Sonar() {
    close(fd);
    std::cout << sensor_name << ": ~Sonar" << std::endl;
}

int Sonar::Init() {
    // 打开串口
    fd = open(dev_name.data(), O_RDWR | O_NOCTTY);
    if (fd < 0) {
        std::cerr << sensor_name << ": Error opening serial port" << std::endl;
        return -1;
    }

    //配置串口参数
    struct termios options;
    if (tcgetattr(fd, &options) != 0) {
        std::cerr << sensor_name << ": Error getting serial port attributes" << std::endl;
        return -1;
    }

    // 设置波特率
    cfsetispeed(&options, B9600);
    cfsetospeed(&options, B9600);

    // 配置控制模式
    options.c_cflag |= (CLOCAL | CREAD);
    options.c_cflag &= ~PARENB; // 无校验位
    options.c_cflag &= ~CSTOPB; // 1个停止位
    options.c_cflag &= ~CSIZE;
    options.c_cflag |= CS8;     // 8位数据位

    // 设置本地模式和接收使能
    options.c_lflag &= ~(ICANON | ECHO | ECHOE | ISIG);
    options.c_oflag &= ~OPOST;

    if (tcsetattr(fd, TCSANOW, &options) != 0) {
        std::cerr << sensor_name << ": Error setting serial port attributes" << std::endl;
        return -1;
    }
    return 0;
}


int Sonar::DataCollection() {
    sendData();
    readData();
    return 0;
}          

int Sonar::DataProcessing() {
    //validateChecksum(buffer)
    if(validateChecksum(buffer))
    {
    //std::cout << "data process!";
    char first_five[6] = {0};
    first_five[5] = '\0';
    if (buffer == nullptr) {
        std::cerr <<sensor_name << ": Error: buffer is null." << std::endl;
        return -1;
    }
    for(int i = 0; i < 6 ;i++){
        std::cout<<first_five[i]<<" ";
    }
    //std::cout<<std::endl;
    std::strncpy(first_five, buffer, 5);

    if (p_sonar_data == nullptr) {
        std::cerr <<sensor_name << ": Error: p_sonar_data is null." << std::endl;
        return -1;
    }
     p_sonar_data->water_depth = std::stof(first_five);
     
    // try {
    //     // 可能抛出异常的代码，如std::stof转换
    //     p_sonar_data->water_depth = std::stof(first_five);
    // } 
    // catch (const std::invalid_argument& e) {
    //     // 处理非数字字符（如"abc"）或空字符串
    //     std::cerr << "无效参数：" << e.what() << std::endl;
    //     p_sonar_data->water_depth = -1;
    //     // return -1;
    // }
    // catch (const std::out_of_range& e) {
    //     // 处理数值超出float范围（如"3.4e39"）
    //     std::cerr << "数值溢出：" << e.what() << std::endl;
    //     // return -1;
    //     p_sonar_data->water_depth = -1;
    // }
    // catch (...) {
    //     // 兜底处理其他未知异常
    //     std::cerr << "未知错误" << std::endl;
    //     // return -1;
    //     p_sonar_data->water_depth = -1;
    // }
    // p_sonar_data->cnt++;

    // //std::cout << "processng success" << std::endl;
    // return 0;
}else{
    std::cout <<"error data"<< std::endl;
    return 0;
}

return 0;
}
int Sonar::DataStorage() {
    // 拷贝p_gnss_data到p_gnss_data_shared
    {
        std::lock_guard<std::mutex> lock(sonar_data_mutex);  // 加互斥锁
        sonar_latest_data = *p_sonar_data;
        sonar_data_cv.notify_one();                          // 通知主线程，此时会释放互斥锁
    }
    
    return 0;
}                       


int Sonar::DataPrint(){
        // //std::cout << "读取到的数据（共 " << bytesRead << " 个字节）: ";

        std::cout << "读取到的水深数据: ";
        for (int i = 0; i < 5; ++i) {
            std::cout << std::hex << static_cast<int>(static_cast<unsigned char>(buffer[i])) << " ";
        }
        std::cout << std::fixed << std::setprecision(3);
        std::cout <<sensor_name << ": water depth: " << p_sonar_data->water_depth <<"m"<< std::endl;
        memset(buffer, 0, sizeof(buffer));
        bytesRead = 0;
        std::cout<<std::endl;
        // for (int i = 0; i < 5; ++i) {
        //    // std::cout << std::hex << static_cast<int>(static_cast<char>(buffer[i])) << " m";
        //     std::cout <<static_cast<char>(buffer[i]);
        // }
        // std::cout <<"m"<<std::endl;
        //sleep(1000);

    return 0;
}



// 发送数据函数
int Sonar::sendData() {
    ssize_t bytesWritten = write(fd, &dataToSend, 1);
    if (bytesWritten != 1) {
        std::cerr << sensor_name << ": Error writing to serial port" << std::endl;
        return -1;
    }
    return 0;
}


int Sonar::readData() {
    fd_set readfds;
    struct timeval timeout;
    FD_ZERO(&readfds);
    FD_SET(fd, &readfds);
    timeout.tv_sec = timeoutSec;
    timeout.tv_usec = 0;

    int activity = select(fd + 1, &readfds, NULL, NULL, &timeout);
    if (activity == -1) {
        std::cerr << sensor_name << ": Error in select" << std::endl;
        return -1;
    } else if (activity == 0) {
        std::cout <<sensor_name<< ": select timeout" << std::endl;
        return 0;
    }

    bytesRead = read(fd, buffer, sizeof(buffer));
    if (bytesRead > 0) {
       
        //std::cout << "read success" << std::endl;
        buffer[bytesRead] = '\0';
        std::cout << "读取到的数据（共 " << bytesRead << " 个字节）: " <<std::endl;
        // std::cout << "水深为： ";
        // for (int i = 0; i < 5; ++i) {
        //    // std::cout << std::hex << static_cast<int>(static_cast<char>(buffer[i])) << " m";
        //     std::cout <<static_cast<char>(buffer[i]);
        // }
        // std::cout <<"m"<<std::endl;
        return 0;
    } else if (bytesRead == 0) {
        std::cout <<sensor_name<< ": no data" << std::endl;
    } else {
        std::cout<<sensor_name<< ": read failed" << std::endl;
        return -1;
    }
    return 0;
}

// 校验和验证
bool validateChecksum(char buffer[]) {

    if (bytesRead != 21) {
        std::cout <<"error length" << std::endl;
        return false; // 数据长度不符合要求
    }
    unsigned char calculatedChecksum = 0;
    // 计算前 20 个字节的校验和
    for (size_t i = 0; i < 20; ++i) {
        calculatedChecksum += static_cast<unsigned char>(buffer[i]);
    }

    // 比较计算得到的校验和与数据中最后一个字节的校验和
    std::cout <<"check " << std::endl;
    return calculatedChecksum == static_cast<unsigned char>(buffer[20]);
}






#else
bool validateChecksum(char buffer[]);
bool inFrame = false;
//std::vector<unsigned char> frameBuffer;
bool parseDataFrame(const std::vector<unsigned char>& frame); // 添加函数声明
size_t bytesRead = 0;
int temperature = 0;
uint16_t depth = 0;
bool wzx = false;
int frameState = 0;
int framedone = 0;
Sonar::Sonar(const std::string& sensor_name_, const int& run_mode_, const std::string& dev_name_) : SensorAbstraction(sensor_name_, run_mode_) {
    dev_name = dev_name_;
    //sensor_data.reserve(512);
    p_sonar_data = std::make_unique<struct sonar_data>();
}

Sonar::~Sonar() {
    close(fd);
    std::cout << sensor_name << ": ~Sonar" << std::endl;
}

int Sonar::Init() {
    // 打开串口
    fd = open(dev_name.data(), O_RDWR | O_NOCTTY);
    if (fd < 0) {
        std::cerr << sensor_name << ": Error opening serial port" << std::endl;
        return -1;
    }

    //配置串口参数
    struct termios options;
    if (tcgetattr(fd, &options) != 0) {
        std::cerr << sensor_name << ": Error getting serial port attributes" << std::endl;
        return -1;
    }

    // 设置波特率
    cfsetispeed(&options, B9600);
    cfsetospeed(&options, B9600);

    // 配置控制模式
    options.c_cflag |= (CLOCAL | CREAD);
    options.c_cflag &= ~PARENB; // 无校验位
    options.c_cflag &= ~CSTOPB; // 1个停止位
    options.c_cflag &= ~CSIZE;
    options.c_cflag |= CS8;     // 8位数据位


    tcsetattr(fd, TCSANOW, &options);
    
    // 清空缓冲区
    tcflush(fd, TCIOFLUSH);



    // 设置本地模式和接收使能
    options.c_lflag &= ~(ICANON | ECHO | ECHOE | ISIG);
    options.c_oflag &= ~OPOST;

    if (tcsetattr(fd, TCSANOW, &options) != 0) {
        std::cerr << sensor_name << ": Error setting serial port attributes" << std::endl;
        return -1;
    }
    return 0;
}


int Sonar::DataCollection() {
    //sendData();
    //readData();
    //bytesRead = read(fd, buffer, sizeof(buffer));
    // if (bytesRead < 0) {
    //             std::cerr << sensor_name << ": Error reading from serial port" << std::endl;
    // }
    // std::cout << "读取到的数据（共 " << bytesRead << " 个字节）: ";
    //bytesRead = read(fd, buffer, sizeof(buffer));
    // if (bytesRead > 0) {
    //     sensor_data.append(buffer, bytesRead);
    // }
    // for (int i = 0; i < bytesRead; ++i) {
    //         // std::cout << std::hex << static_cast<int>(static_cast<char>(buffer[i])) << " m";
    //         std::cout << std::hex << std::setw(2) << std::setfill('0') << static_cast<unsigned int>(static_cast<unsigned char>(buffer[i])) << " ";
    // }
    // std::cout << std::endl;

    //sleep(0.01);
    return 0;
}          

int Sonar::DataProcessing() {
    //validateChecksum(buffer)
//     if(validateChecksum(buffer))
//     {
//         p_sonar_data->temperature = static_cast<int>(buffer[3]);
//         p_sonar_data->water_depth = static_cast<uint16_t>(buffer[4] << 8 | buffer[5]);
//             for (int i = 0; i < 9; ++i) {
//             // std::cout << std::hex << static_cast<int>(static_cast<char>(buffer[i])) << " m";
//             std::cout << std::hex << std::setw(2) << std::setfill('0') << static_cast<unsigned int>(static_cast<unsigned char>(buffer[i])) << " ";
//         }
//         std::cout <<" "<<std::endl;
// }else{
//     std::cout <<"error data"<< std::endl;
//     return 0;
// }
      std::vector<unsigned char> frameBuffer;
      bytesRead = read(fd, buffer, sizeof(buffer));
      if (bytesRead > 0) {
            // 处理接收到的每个字节

            for (int i = 0; i < bytesRead; ++i) {
                unsigned char byte = static_cast<unsigned char>(buffer[i]);
                //std::cout <<"processing"<<std::endl;
                // 帧头检测

                switch (frameState) {
                case 0:  // 等待0x6A
                    if (byte == 0x6A) {
                        frameBuffer.clear();
                        frameBuffer.push_back(byte);
                        frameState = 1;  // 下一状态：等待0x01
                    }
                    break;
                    
                case 1:  // 等待0x01
                    if (byte == 0x01) {
                        frameBuffer.push_back(byte);
                        frameState = 2;  // 下一状态：等待0x06
                    } else {
                        // 不匹配，重置状态
                        frameBuffer.clear();
                        frameState = 0;
                    }
                    break;
                    
                case 2:  // 等待0x06
                    if (byte == 0x06) {
                        frameBuffer.push_back(byte);
                        frameState = 3;  // 帧头完整，开始积累数据
                    } else {
                        // 不匹配，重置状态
                        frameBuffer.clear();
                        frameState = 0;
                    }
                    break;
                    
                case 3:  // 已接收完整帧头，积累数据
                    frameBuffer.push_back(byte);
                    
                    // 检查是否收到完整帧（8字节）
                    if (frameBuffer.size() == 8) {

                            // 解析数据
                            p_sonar_data->temperature = static_cast<int>(frameBuffer[3]);
                            p_sonar_data->water_depth = static_cast<uint16_t>(frameBuffer[4] << 8 | frameBuffer[5]);
                            
                            // 打印帧数据（调试用）
                            // for (int i = 0; i < 8; ++i) {
                            //     std::cout << std::hex << std::setw(2) << std::setfill('0') << static_cast<unsigned int>(frameBuffer[i]) << " ";
                            // }
                            // std::cout << std::endl;
                            // std::cout << std::dec;
                            // std::cout << "温度为: " << p_sonar_data->temperature << "℃" ;

                            // std::cout << "   水深为: " << p_sonar_data->water_depth << "mm" << std::endl;
                            //std::cout << "Process success" << std::endl;
                     
                        framedone = 1;
                        p_sonar_data->cnt++;
                        // 处理完毕，重置状态
                        frameBuffer.clear();
                        frameState = 0;
                    }
                    break;
            }
        }
    } else if (bytesRead < 0) {
        std::cerr << "读取串口数据失败" << std::endl;
    }
    
    return 0;
}


int Sonar::DataStorage() {
    // 拷贝p_gnss_data到p_gnss_data_shared
    if(framedone == 1){
    {
        std::lock_guard<std::mutex> lock(sonar_data_mutex);  // 加互斥锁
        sonar_latest_data = *p_sonar_data;
        sonar_data_cv.notify_one();                          // 通知主线程，此时会释放互斥锁
        framedone = 0;
    }
    }
    return 0;
}                       


int Sonar::DataPrint(){
        if(framedone == 1){
        std::cout << "温度为: " << p_sonar_data->temperature << "℃" ;

        std::cout << "   水深为: " << p_sonar_data->water_depth << "mm" << std::endl;
        framedone = 0;
        }
        // for (int i = 0; i < 5; ++i) {
        //     std::cout << std::hex << static_cast<int>(static_cast<unsigned char>(buffer[i])) << " ";
        // }
        // std::cout << std::fixed << std::setprecision(3);
        // std::cout <<sensor_name << ": water depth: " << p_sonar_data->water_depth <<"m"<< std::endl;
        // memset(buffer, 0, sizeof(buffer));
        // bytesRead = 0;
        // std::cout<<std::endl;
        // for (int i = 0; i < 5; ++i) {
        //    // std::cout << std::hex << static_cast<int>(static_cast<char>(buffer[i])) << " m";
        //     std::cout <<static_cast<char>(buffer[i]);
        // }
        // std::cout <<"m"<<std::endl;
        //sleep(1000);

    return 0;
}

// 校验和验证

const unsigned char Crc8Table[256] = { 
0x00, 0x5e, 0xbc, 0xe2, 0x61, 0x3f, 0xdd, 0x83, 
0xc2, 0x9c, 0x7e, 0x20, 0xa3, 0xfd, 0x1f, 0x41, 
0x9d, 0xc3, 0x21, 0x7f, 0xfc, 0xa2, 0x40, 0x1e, 
0x5f, 0x01, 0xe3, 0xbd, 0x3e, 0x60, 0x82, 0xdc, 
0x23, 0x7d, 0x9f, 0xc1, 0x42, 0x1c, 0xfe, 0xa0, 
0xe1, 0xbf, 0x5d, 0x03, 0x80, 0xde, 0x3c, 0x62, 
0xbe, 0xe0, 0x02, 0x5c, 0xdf, 0x81, 0x63, 0x3d, 
0x7c, 0x22, 0xc0, 0x9e, 0x1d, 0x43, 0xa1, 0xff, 
0x46, 0x18, 0xfa, 0xa4, 0x27, 0x79, 0x9b, 0xc5, 
0x84, 0xda, 0x38, 0x66, 0xe5, 0xbb, 0x59, 0x07, 
0xdb, 0x85, 0x67, 0x39, 0xba, 0xe4, 0x06, 0x58, 
0x19, 0x47, 0xa5, 0xfb, 0x78, 0x26, 0xc4, 0x9a, 
0x65, 0x3b, 0xd9, 0x87, 0x04, 0x5a, 0xb8, 0xe6, 
0xa7, 0xf9, 0x1b, 0x45, 0xc6, 0x98, 0x7a, 0x24, 
0xf8, 0xa6, 0x44, 0x1a, 0x99, 0xc7, 0x25, 0x7b, 
0x3a, 0x64, 0x86, 0xd8, 0x5b, 0x05, 0xe7, 0xb9, 
0x8c, 0xd2, 0x30, 0x6e, 0xed, 0xb3, 0x51, 0x0f, 
0x4e, 0x10, 0xf2, 0xac, 0x2f, 0x71, 0x93, 0xcd, 
0x11, 0x4f, 0xad, 0xf3, 0x70, 0x2e, 0xcc, 0x92, 
0xd3, 0x8d, 0x6f, 0x31, 0xb2, 0xec, 0x0e, 0x50, 
0xaf, 0xf1, 0x13, 0x4d, 0xce, 0x90, 0x72, 0x2c, 
0x6d, 0x33, 0xd1, 0x8f, 0x0c, 0x52, 0xb0, 0xee, 
0x32, 0x6c, 0x8e, 0xd0, 0x53, 0x0d, 0xef, 0xb1, 
0xf0, 0xae, 0x4c, 0x12, 0x91, 0xcf, 0x2d, 0x73, 
0xca, 0x94, 0x76, 0x28, 0xab, 0xf5, 0x17, 0x49, 
0x08, 0x56, 0xb4, 0xea, 0x69, 0x37, 0xd5, 0x8b, 
0x57, 0x09, 0xeb, 0xb5, 0x36, 0x68, 0x8a, 0xd4, 
0x95, 0xcb, 0x29, 0x77, 0xf4, 0xaa, 0x48, 0x16, 
0xe9, 0xb7, 0x55, 0x0b, 0x88, 0xd6, 0x34, 0x6a, 
0x2b, 0x75, 0x97, 0xc9, 0x4a, 0x14, 0xf6, 0xa8, 
0x74, 0x2a, 0xc8, 0x96, 0x15, 0x4b, 0xa9, 0xf7, 
0xb6, 0xe8, 0x0a, 0x54, 0xd7, 0x89, 0x6b, 0x35 
}; 

// CRC-8计算函数
unsigned char Crc8(unsigned char *pcBlock, unsigned char len) 
{ 
    unsigned char crc = 0x00; 
    while(len--) 
        crc = Crc8Table[crc ^ *pcBlock++]; 
    return crc;
}


bool validateChecksum(char buffer[]) {
    
    if(bytesRead != 9){
        std::cerr << "数据帧长度错误,应为9字节" << std::endl;
        return false;
    }

    if (buffer[0] != 0x6A || buffer[1] != 0x01 || buffer[2] != 0x06) {
        std::cerr << "帧头错误,应为6A 01 06" << std::endl;
        return false;
    }

    unsigned char calculatedChecksum = 0;
    // 计算前 8 个字节的CRC-8校验和

    calculatedChecksum = Crc8((unsigned char *)buffer, 8); 

    return calculatedChecksum == static_cast<unsigned char>(buffer[8]);
  
}


// 解析完整的9字节数据帧
bool parseDataFrame(const std::vector<unsigned char>& frame) {
    if (frame.size() != 9) {
        std::cerr << "数据帧长度错误，应为9字节" << std::endl;
        return false;
    }
    if (frame[0] != 0x6A || frame[1] != 0x01 || frame[2] != 0x06) {
        std::cerr << "帧头错误，应为6A 01 06" << std::endl;
        return false;
    }
    
    // 计算前8字节的CRC-8校验和
    unsigned char calculatedCrc = Crc8(const_cast<unsigned char*>(frame.data()), 8);
    if (calculatedCrc != frame[8]) {
        std::cerr << "CRC校验失败: 计算=" << std::hex << (int)calculatedCrc 
                  << " 接收=" << (int)frame[8] << std::endl;
        return false;
    }
    temperature = static_cast<int>(frame[3]);
    depth = static_cast<uint16_t>(frame[4] << 8 | frame[5]);
        for (int i = 0; i < 9; ++i) {
            // std::cout << std::hex << static_cast<int>(static_cast<char>(buffer[i])) << " m";
            std::cout << std::hex << std::setw(2) << std::setfill('0') << static_cast<unsigned int>(static_cast<unsigned char>(frame[i])) << " ";
        }
        std::cout <<" "<<std::endl;
    // // 解析有效数据（示例：提取温度和距离）
    // std::cout << "✓ 有效帧: ";
    // for (unsigned char b : frame) {
    //     std::cout << std::hex << std::setw(2) << std::setfill('0') << (int)b << " ";
    // }
    // std::cout << std::endl;
    
    // // 示例：解析温度（假设第4-5字节为温度值）
    // int temperature = (frame[3] << 8) | frame[4];
    // std::cout << "  温度: " << temperature / 10.0 << "°C" << std::endl;
    
    // // 示例：解析距离（假设第6-7字节为距离值）
    // int distance = (frame[5] << 8) | frame[6];
    // std::cout << "  距离: " << distance << "mm" << std::endl;
    
    return true;
}

#endif